import math

import mozi_utils.geo as geo
from my_envs import etc


def clip(x, max_distance):
    if x >= max_distance:
        return max_distance
    elif x <= -max_distance:
        return -max_distance
    else:
        return x


def restrict(x, y , max_distance):
    d = math.sqrt(x*x + y*y)
    if d <= max_distance:
        return x, y
    else:
        f = (max_distance - 1) /d
        return f * x, f * y


class Agent:
    # 该智能体控制的船只
    x: float

    y: float

    ship_heading: float

    """辅助变量"""
    # 从usv指向目标的向量的角度
    degree_to_goal: float = 0.0

    """本地特征 local state """
    # USV相对于目标的舷角
    relative_bearing: float = 0.0

    # USV与目标的距离 - 围捕距离的阈值
    distance_remaining: float = 0.0

    # usv当前的速度
    current_speed: float = 0.0

    """
        TODO : 上面两个量的变化率，可以简化为相对上一帧的变化量
    """

    # 舷角的变化率
    degree_changing: float = 0.0

    # 距离的变化率
    distance_remaining: float = 0.0

    # 加速度
    speed_changing: float = 0.0

    def update_local_state(self, contact_x, contact_y, pursuit_distance):
        self.degree_to_goal = geo.get_degree(self.y, self.x, contact_y,
                                             contact_x)
        degree = self.ship_heading - self.degree_to_goal
        self.degree_changing = degree - self.relative_bearing
        self.relative_bearing = degree

        distance = geo.get_two_point_distance(self.x, self.y,
                                              contact_x, contact_y) - pursuit_distance
        self.distance_changing = distance - self.distance_remaining
        self.distance_remaining = distance

    def step(self, new_speed, new_heading):
        self.speed_changing = new_speed - self.current_speed
        self.current_speed = (new_speed + 360 )% 360
        self.ship_heading = new_heading
        r = math.radians(new_heading)
        delta_y = new_speed * math.cos(r)
        delta_x = new_speed * math.sin(r)
        self.x = self.x + delta_x
        self.y = self.y + delta_y
        # self.x = clip(self.x, etc.pursuit_distance)
        # self.y = clip(self.y, etc.pursuit_distance)
        self.x, self.y = restrict(self.x, self.y, etc.boundary_distance)

    # 传入USV和目标的信息
    def __init__(self, contact_x, contact_y, x, y, ship_speed, ship_heading, pursuit_distance):
        self.degree_to_goal = geo.get_degree(y, x, contact_y,
                                             contact_x)
        self.relative_bearing = ship_heading - self.degree_to_goal
        self.distance_remaining = geo.get_two_point_distance(x, y,
                                    contact_x, contact_y) - pursuit_distance
        self.current_speed = ship_speed
        # 变化率初始为0
        self.degree_changing = 0
        self.distance_changing = 0
        self.speed_changing = 0
        self.x = x
        self.y = y
        self.ship_heading = ship_heading

    def print_info(self):
        # print("船舰ID：" + ship_id)
        print("----------------------------------------")
        print("当前x："+str(self.x))
        print("当前y："+str(self.y))
        print("剩余围捕距离：" + str(self.distance_remaining))
        print("当前朝向：" + str(self.ship_heading))
        print("degree_to_goal：" + str(self.degree_to_goal))
        print("当前速度：" + str(self.current_speed))
        print("----------------------------------------\n")
        # print("\n")


class EscapeAgent:

    x: float = 0.0
    y: float = 0.0
    speed: float = 0.0
    heading: float = 0.0

    def __init__(self):
        self.x = 0
        self.y = 0
        self.speed = 0
        self.heading = 0

    def step(self, new_speed, new_heading):
        self.speed = new_speed
        self.heading = new_heading
        r = math.radians(new_heading)
        delta_y = new_speed * math.cos(r)
        delta_x = new_speed * math.sin(r)
        self.x = self.x + delta_x
        self.y = self.y + delta_y
        # self.x = clip(self.x, 0.8 * etc.pursuit_distance)
        # self.y = clip(self.y, 0.8 * etc.pursuit_distance)
        self.x, self.y = restrict(self.x, self.y, etc.boundary_distance / 1.5)
